package agent.strategy;
import oracle.MoveTypes;


public class calculateOptimalPath {
	public double EXPLORE_THRESHOLD = .5;
	
	/**
	 * This calculates the optimal path to startRow,startCol assuming perfect information
	 * If information is not available, an estimate needs to be made
	 * @param theBoard
	 * @param terminals
	 * @param walls
	 * @param beliefs
	 * @param explored
	 * @param startRow
	 * @param startCol
	 * @param numMovesLeft
	 * @param penaltyDeath
	 * @param maxRows
	 * @param maxColumns
	 * @param numTurnsRemaining
	 * @param valueOfInformation
	 */
	public nextDirObject getNextDirection(double[][] theBoard, boolean[][] terminals, boolean[][] walls, 
		double[][]beliefs, boolean[][] explored, int startRow, int startCol,
		int numMovesLeft, int penaltyDeath, int maxRows, int maxColumns,
		int numTurnsRemaining, double valueOfInformation){
		if(numMovesLeft == 0){
			return null;
		}
		
		/* this is our dynamic programming solution to path planning
		 * dynBoard[r][c][0] represents the utility of a square with 0 moves remaining
		 * These are calculated as either the death penalty or the terminal value
		 * We also have that dynBoard[r][c][1], you can either kill yourself
		 * or move to an adjacent square\explore*/
		double dynBoard[][][] = new double[maxRows][maxColumns][numMovesLeft+1];
		double valRight, valLeft, valUp, valDown, valExplore;
		double rightA, leftA, downA, upA;
		double[] moveVals = new double[5];
		double probC = .0666666666;
		double attemptR ,attemptL,attemptU ,attemptD; 
		
		// our base 
		for(int r=0; r<maxRows; r++){
			for(int c=0; c<maxColumns;c++){
				if(terminals[r][c]){
					dynBoard[r][c][0] = theBoard[r][c];
				}else{
					dynBoard[r][c][0] = theBoard[r][c] + penaltyDeath;
				}
				System.out.println(r+","+c +  "  with val " + dynBoard[r][c][0]);
			}
		}
		
		// our remaining cases
		for(int round = 1; round <= numMovesLeft; round++){
			for(int r=0; r<maxRows; r++){
				for(int c=0; c<maxColumns;c++){
					// calculate right
					if(c+1 >= maxColumns || walls[r][c+1]){
						rightA = dynBoard[r][c][round - 1] + theBoard[r][c];
						if(terminals[r][c])
							rightA = Math.max(rightA, theBoard[r][c]);
					}else{
						rightA = dynBoard[r][c+1][round - 1] + theBoard[r][c+1];
						if(terminals[r][c+1])
							rightA = Math.max(rightA, theBoard[r][c+1]);
					}
					// calculate left
					if(c-1 < 0 || walls[r][c-1]){
						leftA = dynBoard[r][c][round - 1] + theBoard[r][c];
						if(terminals[r][c])
							leftA = Math.max(leftA, theBoard[r][c]);
					}else{
						leftA = dynBoard[r][c-1][round - 1] + theBoard[r][c-1];
						if(terminals[r][c-1])
							leftA = Math.max(leftA, theBoard[r][c-1]);
					}
					// calculate down
					if(r+1 >= maxRows || walls[r+1][c]){
						downA = dynBoard[r][c][round - 1]+ theBoard[r][c];
						if(terminals[r][c])
							downA = Math.max(downA, theBoard[r][c]);
					}else{
						downA = dynBoard[r+1][c][round - 1] + theBoard[r+1][c];
						if(terminals[r+1][c])
							downA = Math.max(downA, theBoard[r+1][c]);
					}
					// calculate up
					if(r-1 < 0 || walls[r-1][c]){
						upA = dynBoard[r][c][round - 1] + theBoard[r][c];
						if(terminals[r][c])
							upA = Math.max(upA, theBoard[r][c]);
					}else{
						upA = dynBoard[r-1][c][round - 1] + theBoard[r-1][c];
						if(terminals[r-1][c])
							upA = Math.max(upA, theBoard[r-1][c]);
					}
					// calculate explore
					// HAHA this would cause so many errors, we have to model each path separately
					if(upA > 1)
						System.out.println("upa is too big");
					
					// now take max (using probabilities) include possibility of killing self
					attemptR = .80*rightA + probC*leftA + probC*upA + probC*downA;
					attemptL = .80*leftA + probC*rightA + probC*upA + probC*downA;
					attemptU = .80*upA + probC*leftA + probC*rightA + probC*downA;
					attemptD = .80*downA + probC*leftA + probC*upA + probC*rightA;
					moveVals[0] = penaltyDeath;
					moveVals[MoveTypes.RIGHT] = attemptR;
					moveVals[MoveTypes.LEFT] = attemptL;
					moveVals[MoveTypes.UP] = attemptU;
					moveVals[MoveTypes.DOWN] = attemptD;
					int maxPos = 0;
					for(int i = 1; i<5; i++){
						if(moveVals[i] > moveVals[maxPos])
							maxPos = i;
					}
					// position i now holds the maximum position of the optimal move 
					dynBoard[r][c][round] = moveVals[maxPos];
					/* maybe leave this in about if we are on a terminal we may end */
					if(terminals[r][c]){
					//	dynBoard[r][c][round] = Math.max(dynBoard[r][c][round], theBoard[r][c]);
					}
				}
			}
		}
		
		/* now we are asked for a specific position what is the optimal move*/
		if(startCol+1 >= maxColumns || walls[startRow][startCol+1]){
			rightA = dynBoard[startRow][startCol][numMovesLeft - 1] + theBoard[startRow][startCol];
		}else{
			rightA = dynBoard[startRow][startCol+1][numMovesLeft - 1] + theBoard[startRow][startCol+1];
		}
		// calculate left
		if(startCol-1 < 0 || walls[startRow][startCol-1]){
			leftA = dynBoard[startRow][startCol][numMovesLeft - 1] + theBoard[startRow][startCol];
		}else{
			leftA = dynBoard[startRow][startCol-1][numMovesLeft - 1] + theBoard[startRow][startCol-1];
		}
		// startColalstartColulate down
		if(startRow+1 >= maxRows || walls[startRow+1][startCol]){
			downA = dynBoard[startRow][startCol][numMovesLeft - 1] + theBoard[startRow][startCol];
		}else{
			downA = dynBoard[startRow+1][startCol][numMovesLeft - 1] + theBoard[startRow+1][startCol];
		}
		// startColalstartColulate up
		if(startRow-1 < 0 || walls[startRow-1][startCol]){
			upA = dynBoard[startRow][startCol][numMovesLeft - 1] + theBoard[startRow][startCol];
		}else{
			upA = dynBoard[startRow-1][startCol][numMovesLeft - 1] + theBoard[startRow-1][startCol];
		}
		// each move is probabilistic except kill self
		attemptR = .80*rightA + probC*leftA + probC*upA + probC*downA;
		attemptL = .80*leftA + probC*rightA + probC*upA + probC*downA;
		attemptU = .80*upA + probC*leftA + probC*rightA + probC*downA;
		attemptD = .80*downA + probC*leftA + probC*upA + probC*rightA;
		
		System.out.println("R:: " + attemptR + "  L:: " + attemptL + "  D::" + attemptD + "  U:: " + attemptU);
		moveVals[0] = penaltyDeath;
		moveVals[MoveTypes.RIGHT] = attemptR;
		moveVals[MoveTypes.LEFT] = attemptL;
		moveVals[MoveTypes.UP] = attemptU;
		moveVals[MoveTypes.DOWN] = attemptD;
		int maxPos = 0;
		for(int i = 1; i<5; i++){
			if(moveVals[i] > moveVals[maxPos])
				maxPos = i;
		}
		System.out.println(MoveTypes.toString(maxPos));
		System.out.println("The max position is " + maxPos + " with utility " + moveVals[maxPos]);
		nextDirObject toReturn = new nextDirObject();
		toReturn.moveDirection = maxPos;
		toReturn.expectedUtility =  moveVals[maxPos];
		return toReturn;
	}


	/**
	 * This should tell us whether we should explore here and then also where we should then move to.
	 */
	public void getExploreAction(boolean[][] explored, int maxRow, int maxCol, int roundNumberOld, int numTerminalStatesOld,
			int currX, int currY){
		// loop through all of the different paths of up to K explore actions
		// we find which one is the best
		/* calculate the total number of tiles that are explored */
		int numExplored = getNumUnexplored(explored,0,0, maxRow, maxCol, maxRow*maxCol+2);
		if(explored[0][0])
			numExplored++;
		int totalSquares = maxRow * maxCol;
		/* this is the percentage of the map that is explored */
		double percentageExplored = numExplored / (maxRow*maxCol *1.0);
		/* see how far a look around move reveals */
		int manhattan = getMan(maxRow, maxCol);
		/* this is the number of terminal states that we know about */
		int numTerminalStates = 0;
		int roundNumber = 0;
		
		int theX = 0; int theY = 0; // the (x,y) that we might explore from
		int maxPossExplore = getNumUnexplored(new boolean[maxRow][maxCol],theX, theY, maxRow, maxCol, manhattan);
		int numWouldExplore = getNumUnexplored(explored, theX, theY, maxRow, maxCol, manhattan);
		/* if we are above our threshold then we just blindly explore */
		if(((double)numWouldExplore) / (maxPossExplore * 1.0) > EXPLORE_THRESHOLD){
			System.out.println("You should explore");
		}
		
		int numMoves = (int)(manhattan);
		int walkAwayValue = (int)(manhattan *1.5);
		
		int bestMove = MoveTypes.LOOK_AROUND;
		double bestOutput = 0;
		/* otherwise we try to maximize our expected exploration */
		for(int r=0; r< maxRow; r++){
			for(int c=0; c< maxCol; c++){
				double probReachSquare = this.probReachSquareFromTo(theX, theY, r, c, numMoves, maxRow, maxCol).expectedUtility;
				int numExpIfMove = this.getNumUnexplored(explored, r, c, maxRow, maxCol, manhattan);
				int maxPossIfExp = this.getNumUnexploredIfExplore(explored, r, c, maxRow, maxCol, manhattan, walkAwayValue);
				double functVal = this.getFunction(percentageExplored, totalSquares, numTerminalStates, 
						probReachSquare, numExpIfMove, maxPossIfExp, walkAwayValue, maxPossExplore, roundNumber);
				if(functVal > bestOutput){
					bestOutput = functVal;
					bestMove = this.probReachSquareFromTo(theX, theY, r, c, numMoves, maxRow, maxCol).moveDirection;
				}	
			}
		}
		
		System.out.println("The explore move is " +  MoveTypes.toString(bestMove) + "  with utility " + bestOutput);
		
	}
	
	private double getFunction(double percentageExplored, int totalSquares, int numTerminalStates,
			double probReachSquare, int addExplored, int maxExpReach, int walkAwayValue, int maxPossExplore,
			int roundNumber){
		if(true){
			return probReachSquare*walkAwayValue*(addExplored + maxExpReach * .3)/((double) maxPossExplore) * Math.exp(-1*numTerminalStates);
		}
		return 0;
	}
	
	/**
	 * returns the manhattan distance affected by the explore action
	 * @param maxRow
	 * @param maxCol
	 * @return
	 */
	public int getMan(int maxRow, int maxCol){
		return (int) Math.ceil((maxRow * maxCol)/100.0);
	}
	
	/**
	* This calculates the number of unexplored squares that will be revealed by an explore action
	 * @param explored A boolean[][] where true means square is revealed
	 * @param row The row from which we are exploring
	 * @param col THe column from which we are exploring
	 * @param maxRow
	 * @param maxCol
	 * @param manhattan
	 * @return
	 */
	public int getNumUnexplored(boolean[][] explored, int row, int col, int maxRow, int maxCol, int manhattan){
		int num = 0;
		for(int r=0; r<maxRow; r++){
			for(int c=0; c< maxCol; c++){
				if(r==row && c==col)
					continue;
				if(!explored[r][c] && (Math.abs(c-col)+Math.abs(r-row))<=manhattan)
					num++;
			}
		}
		return num;
	}
	
	/**
	 * This says explore at (row,col) and then see if we walk away a distance at most walkAwayValue, how
	 * many unexplored squares do we get
	 * @param explored	A boolean representation of where we have explored
	 * @param row The row that we are exploring  
	 * @param col The column that we are exploring 
	 * @param maxRow The max number of rows of our board
	 * @param maxCol The max number of columns of our board
	 * @param manhattan The manhattan distance of the effect of an explore action
	 * @param walkAwayVal How far we are willing to walk away
	 * @return
	 */
	public int getNumUnexploredIfExplore(boolean explored[][], int row, int col, int maxRow, 
			int maxCol, int manhattan, int walkAwayVal){
		boolean newExp[][] = new boolean[maxRow][maxCol];
		for(int r=0; r<maxRow; r++){
			for(int c=0; c< maxCol; c++){
				newExp[r][c] = explored[r][c];
			}
		}
		
		int myMax = 0;
		int goodR = -1; int goodC = -1;
		int temp;
		// now find the maximum
		for(int r=0; r<maxRow; r++){
			for(int c=0; c< maxCol; c++){
				if((Math.abs(c-col)+Math.abs(r-row))<=manhattan){
					// count number of unexplored here
					temp = this.getNumUnexplored(newExp, r, c, maxRow, maxCol, manhattan);
					if(temp > myMax){
						myMax = temp;
						goodR = r;
						goodC = c;
					}
				}
			}
		}
		
		return 0;
	}
	
	
	/**
	 * This calculates the probablity of reaching a square in at most numMoves if the first move
	 * is done as specified in the nextDirObject
	 * @param startRow	The starting row of the agent
	 * @param startCol The starting column of the agent
	 * @param endR The ending row of the agent
	 * @param endC The ending column of the agent
	 * @param numMoves The max number of moves to reach the square
	 * @param maxRow The max number of rows in the board
	 * @param maxCol The max number of columns in the board
	 * @return The [0,1] probabilty of getting to the destination
	 */
	public nextDirObject probReachSquareFromTo(int startRow, int startCol, int endR, int endC, int numMoves, int maxRow, int maxCol){
		double[][] theBoard = new double[maxRow][maxCol];
		boolean[][] walls = new boolean[maxRow][maxCol];
		boolean[][] terminals = new boolean[maxRow][maxCol];
		terminals[endR][endC] = true;
		theBoard[endR][endC] = 1;
		nextDirObject myR = this.BBgetNextDirection(theBoard, terminals, walls, null, null, startRow, startCol, numMoves, 0, maxRow, maxCol, 0, 0);
		// myR now says that moving in a certain direction maximizes prob at this amount
		return myR;
	}
	
	/**
	 * This is used to calculate probabilty of getting to a square
	 * The square that we are trying to get to is initialized with 1
	 * The value of all other squares are assigned a value of 0
	 * @param theBoard	double filled with values, all = 0 except square trying to get to
	 * @param terminals	true if there is a terminal at a location
	 * @param walls	true if there is a wall at a location
	 * @param beliefs	true if we know that is the actual tile
	 * @param explored	true if we have explored this tile
	 * @param startRow	The starting row
	 * @param startCol The starting column
	 * @param numMovesLeft The maximum number of moves that we have to reach there
	 * @param penaltyDeath	This should be set to 0
	 * @param maxRows The max number of rows in our world
	 * @param maxColumns The max number of columns in our map
	 * @param numTurnsRemaining	Not used
	 * @param valueOfInformation Not used
	 * @return
	 */
	private nextDirObject BBgetNextDirection(double[][] theBoard, boolean[][] terminals, boolean[][] walls, 
			double[][]beliefs, boolean[][] explored, int startRow, int startCol,
			int numMovesLeft, int penaltyDeath, int maxRows, int maxColumns,
			int numTurnsRemaining, double valueOfInformation){
			if(numMovesLeft == 0){
				return null;
			}
			
			
			/* this is our dynamic programming solution to path planning
			 * dynBoard[r][c][0] represents the utility of a square with 0 moves remaining
			 * These are calculated as either the death penalty or the terminal value
			 * We also have that dynBoard[r][c][1], you can either kill yourself
			 * or move to an adjacent square\explore*/
			double dynBoard[][][] = new double[maxRows][maxColumns][numMovesLeft+1];
			double valRight, valLeft, valUp, valDown, valExplore;
			double rightA, leftA, downA, upA;
			double[] moveVals = new double[5];
			double probC = .0666666666;
			double attemptR ,attemptL,attemptU ,attemptD; 
			
			// our base 
			for(int r=0; r<maxRows; r++){
				for(int c=0; c<maxColumns;c++){
					if(terminals[r][c]){
						dynBoard[r][c][0] = theBoard[r][c];
					}else{
						dynBoard[r][c][0] = theBoard[r][c] + penaltyDeath;
					}
				}
			}
			
			// our remaining cases
			for(int round = 1; round <= numMovesLeft; round++){
				for(int r=0; r<maxRows; r++){
					for(int c=0; c<maxColumns;c++){
						// calculate right
						if(c+1 >= maxColumns || walls[r][c+1]){
							rightA = dynBoard[r][c][round - 1] + theBoard[r][c];
							if(terminals[r][c])
								rightA = Math.max(rightA, theBoard[r][c]);
						}else{
							rightA = dynBoard[r][c+1][round - 1] + theBoard[r][c+1];
							if(terminals[r][c+1])
								rightA = Math.max(rightA, theBoard[r][c+1]);
						}
						// calculate left
						if(c-1 < 0 || walls[r][c-1]){
							leftA = dynBoard[r][c][round - 1] + theBoard[r][c];
							if(terminals[r][c])
								leftA = Math.max(leftA, theBoard[r][c]);
						}else{
							leftA = dynBoard[r][c-1][round - 1] + theBoard[r][c-1];
							if(terminals[r][c-1])
								leftA = Math.max(leftA, theBoard[r][c-1]);
						}
						// calculate down
						if(r+1 >= maxRows || walls[r+1][c]){
							downA = dynBoard[r][c][round - 1]+ theBoard[r][c];
							if(terminals[r][c])
								downA = Math.max(downA, theBoard[r][c]);
						}else{
							downA = dynBoard[r+1][c][round - 1] + theBoard[r+1][c];
							if(terminals[r+1][c])
								downA = Math.max(downA, theBoard[r+1][c]);
						}
						// calculate up
						if(r-1 < 0 || walls[r-1][c]){
							upA = dynBoard[r][c][round - 1] + theBoard[r][c];
							if(terminals[r][c])
								upA = Math.max(upA, theBoard[r][c]);
						}else{
							upA = dynBoard[r-1][c][round - 1] + theBoard[r-1][c];
							if(terminals[r-1][c])
								upA = Math.max(upA, theBoard[r-1][c]);
						}
						// calculate explore
						// HAHA this would cause so many errors, we have to model each path separately
						if(upA > 1)
							upA = 1;
						if(leftA > 1)
							leftA = 1;
						if(rightA > 1)
							rightA = 1;
						if(downA > 1)
							downA = 1;
						
						// now take max (using probabilities) include possibility of killing self
						attemptR = .80*rightA + probC*leftA + probC*upA + probC*downA;
						attemptL = .80*leftA + probC*rightA + probC*upA + probC*downA;
						attemptU = .80*upA + probC*leftA + probC*rightA + probC*downA;
						attemptD = .80*downA + probC*leftA + probC*upA + probC*rightA;
						moveVals[0] = penaltyDeath;
						moveVals[MoveTypes.RIGHT] = attemptR;
						moveVals[MoveTypes.LEFT] = attemptL;
						moveVals[MoveTypes.UP] = attemptU;
						moveVals[MoveTypes.DOWN] = attemptD;
						int maxPos = 0;
						for(int i = 1; i<5; i++){
							if(moveVals[i] > moveVals[maxPos])
								maxPos = i;
						}
						// position i now holds the maximum position of the optimal move 
						dynBoard[r][c][round] = moveVals[maxPos];
						/* maybe leave this in about if we are on a terminal we may end */
						if(terminals[r][c]){
						//	dynBoard[r][c][round] = Math.max(dynBoard[r][c][round], theBoard[r][c]);
						}
					}
				}
			}
			
			/* now we are asked for a specific position what is the optimal move*/
			if(startCol+1 >= maxColumns || walls[startRow][startCol+1]){
				rightA = dynBoard[startRow][startCol][numMovesLeft - 1] + theBoard[startRow][startCol];
			}else{
				rightA = dynBoard[startRow][startCol+1][numMovesLeft - 1] + theBoard[startRow][startCol+1];
			}
			// calculate left
			if(startCol-1 < 0 || walls[startRow][startCol-1]){
				leftA = dynBoard[startRow][startCol][numMovesLeft - 1] + theBoard[startRow][startCol];
			}else{
				leftA = dynBoard[startRow][startCol-1][numMovesLeft - 1] + theBoard[startRow][startCol-1];
			}
			// startColalstartColulate down
			if(startRow+1 >= maxRows || walls[startRow+1][startCol]){
				downA = dynBoard[startRow][startCol][numMovesLeft - 1] + theBoard[startRow][startCol];
			}else{
				downA = dynBoard[startRow+1][startCol][numMovesLeft - 1] + theBoard[startRow+1][startCol];
			}
			// startColalstartColulate up
			if(startRow-1 < 0 || walls[startRow-1][startCol]){
				upA = dynBoard[startRow][startCol][numMovesLeft - 1] + theBoard[startRow][startCol];
			}else{
				upA = dynBoard[startRow-1][startCol][numMovesLeft - 1] + theBoard[startRow-1][startCol];
			}
			// each move is probabilistic except kill self
			attemptR = .80*rightA + probC*leftA + probC*upA + probC*downA;
			attemptL = .80*leftA + probC*rightA + probC*upA + probC*downA;
			attemptU = .80*upA + probC*leftA + probC*rightA + probC*downA;
			attemptD = .80*downA + probC*leftA + probC*upA + probC*rightA;
			
			System.out.println("R:: " + attemptR + "  L:: " + attemptL + "  D::" + attemptD + "  U:: " + attemptU);
			moveVals[0] = penaltyDeath;
			moveVals[MoveTypes.RIGHT] = attemptR;
			moveVals[MoveTypes.LEFT] = attemptL;
			moveVals[MoveTypes.UP] = attemptU;
			moveVals[MoveTypes.DOWN] = attemptD;
			int maxPos = 0;
			for(int i = 1; i<5; i++){
				if(moveVals[i] > moveVals[maxPos])
					maxPos = i;
			}
			System.out.println(MoveTypes.toString(maxPos));
			System.out.println("The max position is " + maxPos + " with utility " + moveVals[maxPos]);
			nextDirObject toReturn = new nextDirObject();
			toReturn.moveDirection = maxPos;
			toReturn.expectedUtility =  moveVals[maxPos];
			return toReturn;
		}

	

	
}
